﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Boids {
    class Bird {
        private static float eps = 1.0e-3f;

        private Vector3 position;
        private Vector3 velocity;
        private Vector3 acceleration;
        private Vector3 direction;
        private float algK, chsK, sprK;
        private float velocityMax, accelerationMax, angleMax, distanceMax;
        private Vector3 bbMax, bbMin;
        private float accelerationVelocity;

        public Bird(Vector3 position, Vector3 velocity) {
            this.position = position;
            this.velocity = velocity;
            this.acceleration = new Vector3();

            velocityMax = 0.4f;
            accelerationMax = 1.0f;
            angleMax = -0.7f;
            distanceMax = 2.0f;
            accelerationVelocity = 0.2f;
            
            algK = 4.0f;
            chsK = 2.0f;
            sprK = 1.0f;

            bbMax = new Vector3(3.0f, 3.5f, eps);
            bbMin = new Vector3(-3.0f, -3.5f, -eps);

            step(0);
        }
        public Vector3 step(float dt) {
            if (position.X < bbMin.X)
                acceleration.X += accelerationVelocity;
            if (position.Y < bbMin.Y)
                acceleration.Y += accelerationVelocity;
            if (position.Z < bbMin.Z)
                acceleration.Z += accelerationVelocity;
            if (position.X > bbMax.X)
                acceleration.X -= accelerationVelocity;
            if (position.Y > bbMax.Y)
                acceleration.Y -= accelerationVelocity;
            if (position.Z > bbMax.Z)
                acceleration.Z -= accelerationVelocity;

            float vl = velocity.Length();
            if (vl > velocityMax)
                velocity *= velocityMax / vl;
            position += velocity * dt;

            float al = acceleration.Length();
            if (al > accelerationMax)
                acceleration *= accelerationMax / al;
            velocity += acceleration * dt;

            direction = Vector3.Normalize(velocity);

            return position;
        }
        public void sense(Bird[] birds) {
            List<Bird> birdsInSight = new List<Bird>();

            foreach (Bird b in birds) {
                if (b == this)
                    continue;
                float dist = Vector3.Distance(b.position, position);
                if (dist > distanceMax)
                    continue;
                float cos = Vector3.Dot(b.direction, direction);
                if (cos < angleMax)
                    continue;
                birdsInSight.Add(b);
            }
            if (birdsInSight.Count <= 0)
                return;

            Bird[] neighbors = birdsInSight.ToArray();
            acceleration = align(neighbors) + cohere(neighbors) + separate(neighbors);
        }
        public Vector3 align(Bird[] birds) {
            Vector3 alignment = new Vector3();
            foreach (Bird b in birds) {
                alignment += b.velocity;
            }
            alignment /= birds.Length;
            alignment -= velocity;
            alignment *= algK;
            return alignment;
        }
        public Vector3 cohere(Bird[] birds) {
            Vector3 cohesion = new Vector3();
            foreach (Bird b in birds) {
                cohesion += b.position;
            }
            cohesion /= birds.Length;
            cohesion -= position;
            cohesion *= chsK;
            return cohesion;
        }
        public Vector3 separate(Bird[] birds) {
            Vector3 separation = new Vector3();
            foreach (Bird b in birds) {
                Vector3 dp = position - b.position;
                float d = dp.Length();
                float w = Math.Max(0, 1 - d / (sprK * distanceMax));
                dp.Normalize();
                separation += dp * w;
            }
            return separation;
        }
    }
}
